#include "PwmOut.h"
#include "wiring_private.h"
#include "pins_arduino.h"
//#include "avr/iom328p.h"
//#include "PwmName.h"

#include "wiring_private.h"
#include "pins_arduino.h"
#include <avr/pgmspace.h>
#include <avr/io.h>
#include <avr/interrupt.h>

#include "binary.h"
// 8 -> 3
// 2->  5
// 1->  6
// 3->  9
// 4->  10
// 7->  11
 /*if(_val <0){
        digitalWrite(_pin, LOW);
        return;
    }else if( _val >254)
    {
        digitalWrite(_pin, HIGH);
        return;
    }*/
PwmOut::PwmOut(PinName pin,uint8_t val):_pin(pin),_val(0),_timer_nx(0)
{
    _timer_nx = digitalPinToTimer(_pin);
    
    pinMode(_pin,OUTPUT);

	_fixed_freq_name = FREQ_NONE;
	_duty_cycle = -1;
    _any_freq = -1;
}
   
/*
void PwmOut::write (float value)
{   
    _val = value*(uint8_t)255;


    switch(_timer_nx)
    {
        #if defined(TCCR0A) && defined(COM0A1)
        case TIMER0A:
            // connect pwm to pin on timer 0, channel A
            sbi(TCCR0A, COM0A1);
            OCR0A = _val; // set pwm duty
            break;
        #endif

        #if defined(TCCR0A) && defined(COM0B1)
        case TIMER0B:
            // connect pwm to pin on timer 0, channel B
            sbi(TCCR0A, COM0B1);
            OCR0B = _val; // set pwm duty
            break;
        #endif

        #if defined(TCCR1A) && defined(COM1A1)
        case TIMER1A:
            // connect pwm to pin on timer 1, channel A
            sbi(TCCR1A, COM1A1);
            OCR1A = _val; // set pwm duty
            break;
        #endif

        #if defined(TCCR1A) && defined(COM1B1)
        case TIMER1B:
            // connect pwm to pin on timer 1, channel B
            sbi(TCCR1A, COM1B1);
            OCR1B = _val; // set pwm duty
            break;
        #endif
        #if defined(TCCR2A) && defined(COM2A1)
        case TIMER2A:
            // connect pwm to pin on timer 2, channel A
            sbi(TCCR2A, COM2A1);
            OCR2A = _val; // set pwm duty
            break;
        #endif

        #if defined(TCCR2A) && defined(COM2B1)
        case TIMER2B:
            // connect pwm to pin on timer 2, channel B
            sbi(TCCR2A, COM2B1);
            OCR2B = _val; // set pwm duty
            break;
        #endif
        case NOT_ON_TIMER:
        default:break;
    }
}
*/
void PwmOut::write(float duty_cycle)
{   
	// if( _duty_cycle ==  duty_cycle){
	// 	return;
	// }

	_duty_cycle = duty_cycle;

    switch (_fixed_freq_name)
    {
    case FREQ_32KHZ:pwm_adjust_32khz_duty_cycle(_timer_nx,duty_cycle); break;
    case FREQ_4KHZ: pwm_adjust_4khz_duty_cycle(_timer_nx,duty_cycle); break;
    case FREQ_500HZ:pwm_adjust_500hz_duty_cycle(_timer_nx,duty_cycle); break;
    case FREQ_125HZ:pwm_adjust_125hz_duty_cycle(_timer_nx,duty_cycle); break;
	case FREQ_31HZ:pwm_adjust_31hz_duty_cycle(_timer_nx,duty_cycle); break;
    case FREQ_ANY:  this->write(duty_cycle);break;
    default:break;
    }
}
void PwmOut::set_frequency(PwmFreqName freq_name,float duty_cycle)
{
	// if(_fixed_freq_name==freq_name){
	// 	return;
	// }
    
	_fixed_freq_name=freq_name;

    switch (_fixed_freq_name)
    {
    case FREQ_32KHZ:
    {
        pwm_adjust_frequency_32khz(_timer_nx,duty_cycle);
    }
    break;
    case FREQ_4KHZ:
    {
        pwm_adjust_frequency_4khz(_timer_nx,duty_cycle);
    }
    break;
    case FREQ_500HZ:
    {
        pwm_adjust_frequency_500hz(_timer_nx,duty_cycle);
    }
    break;
    case FREQ_125HZ:
    {
        pwm_adjust_frequency_125hz(_timer_nx,duty_cycle);
    }
    break;
    case FREQ_31HZ:
    {
		pwm_adjust_frequency_31hz(_timer_nx,duty_cycle);
    }
    break;
	case FREQ_ANY:
    {
		
    }
    break;
    default:break;
    }
}
void PwmOut::set_frequency(uint32_t freq,float duty_cycle)
{	
	// if(_any_freq!=freq){
		_any_freq=freq;
		pwm_adjust_any_freq(_timer_nx,_any_freq,duty_cycle);
	// }
}
/*
void PwmOut::period (float seconds)
{

}
void PwmOut::period_ms (int ms)
{

}
void PwmOut::period_us (int us)
{

}
void PwmOut::period_ns (int ns)
{

}*/
void sd(uint8_t val)
{
    switch(1){
        #if defined(TCCR0) && defined(COM00) && !defined(__AVR_ATmega8__)
			case TIMER0A:
				// connect pwm to pin on timer 0
				sbi(TCCR0, COM00);
				OCR0 = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR0A) && defined(COM0A1)
			case TIMER0A:
				// connect pwm to pin on timer 0, channel A
				sbi(TCCR0A, COM0A1);
				OCR0A = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR0A) && defined(COM0B1)
			case TIMER0B:
				// connect pwm to pin on timer 0, channel B
				sbi(TCCR0A, COM0B1);
				OCR0B = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR1A) && defined(COM1A1)
			case TIMER1A:
				// connect pwm to pin on timer 1, channel A
				sbi(TCCR1A, COM1A1);
				OCR1A = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR1A) && defined(COM1B1)
			case TIMER1B:
				// connect pwm to pin on timer 1, channel B
				sbi(TCCR1A, COM1B1);
				OCR1B = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR1A) && defined(COM1C1)
			case TIMER1C:
				// connect pwm to pin on timer 1, channel C
				sbi(TCCR1A, COM1C1);
				OCR1C = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR2) && defined(COM21)
			case TIMER2:
				// connect pwm to pin on timer 2
				sbi(TCCR2, COM21);
				OCR2 = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR2A) && defined(COM2A1)
			case TIMER2A:
				// connect pwm to pin on timer 2, channel A
				sbi(TCCR2A, COM2A1);
				OCR2A = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR2A) && defined(COM2B1)
			case TIMER2B:
				// connect pwm to pin on timer 2, channel B
				sbi(TCCR2A, COM2B1);
				OCR2B = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR3A) && defined(COM3A1)
			case TIMER3A:
				// connect pwm to pin on timer 3, channel A
				sbi(TCCR3A, COM3A1);
				OCR3A = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR3A) && defined(COM3B1)
			case TIMER3B:
				// connect pwm to pin on timer 3, channel B
				sbi(TCCR3A, COM3B1);
				OCR3B = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR3A) && defined(COM3C1)
			case TIMER3C:
				// connect pwm to pin on timer 3, channel C
				sbi(TCCR3A, COM3C1);
				OCR3C = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR4A)
			case TIMER4A:
				//connect pwm to pin on timer 4, channel A
				sbi(TCCR4A, COM4A1);
				#if defined(COM4A0)		// only used on 32U4
				cbi(TCCR4A, COM4A0);
				#endif
				OCR4A = val;	// set pwm duty
				break;
			#endif
			
			#if defined(TCCR4A) && defined(COM4B1)
			case TIMER4B:
				// connect pwm to pin on timer 4, channel B
				sbi(TCCR4A, COM4B1);
				OCR4B = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR4A) && defined(COM4C1)
			case TIMER4C:
				// connect pwm to pin on timer 4, channel C
				sbi(TCCR4A, COM4C1);
				OCR4C = val; // set pwm duty
				break;
			#endif
				
			#if defined(TCCR4C) && defined(COM4D1)
			case TIMER4D:				
				// connect pwm to pin on timer 4, channel D
				sbi(TCCR4C, COM4D1);
				#if defined(COM4D0)		// only used on 32U4
				cbi(TCCR4C, COM4D0);
				#endif
				OCR4D = val;	// set pwm duty
				break;
			#endif

							
			#if defined(TCCR5A) && defined(COM5A1)
			case TIMER5A:
				// connect pwm to pin on timer 5, channel A
				sbi(TCCR5A, COM5A1);
				OCR5A = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR5A) && defined(COM5B1)
			case TIMER5B:
				// connect pwm to pin on timer 5, channel B
				sbi(TCCR5A, COM5B1);
				OCR5B = val; // set pwm duty
				break;
			#endif

			#if defined(TCCR5A) && defined(COM5C1)
			case TIMER5C:
				// connect pwm to pin on timer 5, channel C
				sbi(TCCR5A, COM5C1);
				OCR5C = val; // set pwm duty
				break;
			#endif

			case NOT_ON_TIMER:
			default:break;
				
		}
}